//VERSION 1.0 - Flown on Mini Bexus 26. Sept 2012 to ~10km alt
#define DEBUG_WP 0

#define VX_FLIGHTPLAN_NO_GS_FLYHOME_TIME 60000 // in [ms], after how long without GS we set flightcase 100

void vx_flightplan()
{
  
  int vx_modechannelin = APM_RC.InputCh(7);
  
  if(vx_modechannelin >= 1800)
  {
    vx_g_transmitter_flightphase = 1;
  }
  if(vx_modechannelin < 1800)
  {
    vx_g_transmitter_flightphase = 2;    
  }
  if(vx_modechannelin < 1600)
  {
    vx_g_transmitter_flightphase = vx_g_last_autonomous_mode;
  }
  if(vx_modechannelin < 1200)
  {
    vx_g_transmitter_flightphase = 100;
  }
  
  
  
  //if(vx_modechannelin < 1500)
  //{
  //  vx_g_transmitter_flightphase = 10;
  //}
  //if(vx_modechannelin < 1400)
  //{
  //  vx_g_transmitter_flightphase = 10;
  //}
   
  
  // Check if transmitter has changed flightphase
//  if (vx_g_transmitter_flightphase != vx_g_transmitter_flightphase_last)
//  {
//    vx_g_flightphase = vx_g_transmitter_flightphase;    
//    vx_g_transmitter_flightphase_last = vx_g_transmitter_flightphase;
//  }
  
  // Check so that flightphase isn't 1 or 2 (RC is in control)
  if((vx_g_flightphase != 1) && (vx_g_flightphase != 2)) {
    // If connection lost to ground station -> set flightcase fly to homebase
    if((millis() - vx_g_tele_last_received_time) > VX_FLIGHTPLAN_NO_GS_FLYHOME_TIME) {
      vx_g_flightphase = 100;
      
      // Use navigation for heading, but keep pitch at -10 degrees.
      vx_g_use_fixed_target_heading = false;
      vx_g_use_fixed_flightpathangle = true;
      vx_g_fixed_flightpathangle = -10.0;
    }
  }
  
  
  switch(vx_g_flightphase)
  {
  case(1): //MANUAL
        vx_g_flag_stabilize = false;
//        Serial.print("Stabilize Flag set to: ");
//        Serial.println(vx_g_flag_stabilize);
//	APM_RC.OutputCh(0, APM_RC.InputCh(0));
//	APM_RC.OutputCh(1, APM_RC.InputCh(1));

        break;

  case(2)://FBW
        if (!vx_g_flag_stabilize)
        {
          vx_g_setmiddledeltawing();
          vx_g_setdiffdeltawing();
          vx_g_flag_stabilize = true;  
        }
        
	vx_flightphase_fbw();
	break;

  case(3)://guided
	if(vx_navigation(vx_g_wp_guided.lat, vx_g_wp_guided.lon, vx_g_wp_guided.alt))
        {
          vx_g_flightphase++;
          #if DEBUG_WP == 1
          Serial3.println("WP Guided reached");
          #endif
        }
	break;

  case(5)://GS FBW
	if(vx_flighphase_gsfbw())
        {
          vx_g_flightphase = vx_g_last_autonomous_mode;
          #if DEBUG_WP == 1
          Serial3.println("GS FBW Mode ended");
          Serial.println("GS FBW Mode ended");
          #endif
        }
	break;

  case(6):// DROP
        vx_g_nav_pidlistinuse = &vx_g_nav_pidlist_dive; 
        vx_g_stab_pidlistinuse = &vx_g_stab_pidlist_dive; 
        if((vx_g_highest_altitude - vx_g_current_pos.alt) > VX_DROP_RECOGNITION_ALTITUDE) //VX_DROP_RECOGNITION_ALTITUDE [cm]
        {
          vx_launch_auto();
          vx_g_flightphase = 100;
        }
	break;

  case(7):// Neutral
        vx_g_nav_pidlistinuse = &vx_g_nav_pidlist_dive; 
        vx_g_stab_pidlistinuse = &vx_g_stab_pidlist_dive; 
        if (!vx_neutralrudders)
        {
          vx_neutralrudders_timer = millis();
          vx_neutralrudders = true;
        }
        if((millis() - vx_neutralrudders_timer) > 15000)
        {
          vx_g_flightphase = 100;
          vx_neutralrudders = false;
        }
        
        break;







case(10):
	if(vx_navigation(678910593,210843178, vx_g_home.alt + 150000))
        {
          #if DEBUG_WP == 1
          Serial3.print("WP reached: ");
          Serial3.println(vx_g_flightphase);
          #endif
          vx_g_flightphase++;
        }
	break;

case(11):
	if(vx_navigation(678913824,211006257, vx_g_home.alt + 150000))
        {
          #if DEBUG_WP == 1
          Serial3.print("WP reached: ");
          Serial3.println(vx_g_flightphase);
          #endif
          vx_g_flightphase++;        
        }
	break;

case(12):
	if(vx_navigation(678845330,211016556, vx_g_home.alt + 150000))
        {
          #if DEBUG_WP == 1
          Serial3.print("WP reached: ");
          Serial3.println(vx_g_flightphase);
          #endif
          vx_g_flightphase++;        
        }
	break;

case(13):
	if(vx_navigation(678848561,210851761, vx_g_home.alt + 150000))
        {
          #if DEBUG_WP == 1
          Serial3.print("WP reached: ");
          Serial3.println(vx_g_flightphase);
          #endif
          vx_g_flightphase = 10;        
        }
	break;











case(20):
	if(vx_navigation(678921253,210682675, vx_g_home.alt + 150000))
        {
          #if DEBUG_WP == 1
          Serial3.print("WP reached: ");
          Serial3.println(vx_g_flightphase);
          #endif
          vx_g_flightphase++;
        }
	break;

case(21):
	if(vx_navigation(678913824,211006257, vx_g_home.alt + 150000))
        {
          #if DEBUG_WP == 1
          Serial3.print("WP reached: ");
          Serial3.println(vx_g_flightphase);
          #endif
          vx_g_flightphase++;        
        }
	break;

case(22):
	if(vx_navigation(678845330,211016556, vx_g_home.alt + 150000))
        {
          #if DEBUG_WP == 1
          Serial3.print("WP reached: ");
          Serial3.println(vx_g_flightphase);
          #endif
          vx_g_flightphase++;        
        }
	break;

case(23):
	if(vx_navigation(678827234,210670659, vx_g_home.alt + 150000))
        {
          #if DEBUG_WP == 1
          Serial3.print("WP reached: ");
          Serial3.println(vx_g_flightphase);
          #endif
          vx_g_flightphase = 10;        
        }
	break;


















case(100):
	if(vx_navigation(vx_g_home.lat, vx_g_home.lon, vx_g_home.alt + 150000))
        {
          #if DEBUG_WP == 1
          Serial3.print("WP reached: ");
          Serial3.println(vx_g_flightphase);
          #endif
          vx_g_flightphase = 10;        
        }
	break;









default:
        vx_g_flightphase = 100;
        break;
  }
  
  
  if ((vx_g_flightphase != 1) && (vx_g_flightphase != 2) && (vx_g_flightphase != 5))
  {
    vx_g_last_autonomous_mode = vx_g_flightphase;          
  }
  
}

